﻿#include "commander_old.h"
#include <QDebug>
#include <QFile>

#define BUFF_MAX_LEN (256)
static quint8 s_buff[BUFF_MAX_LEN] = {0};

CommanderOld* CommanderOld::s_instance = nullptr;

static void getCrc16X(quint8* buff, quint8 bufLen, quint8* buffOut)
{
    quint8 i, j;
    quint16 crc_accum = 0;
    quint16 crc_table[256] = {
        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};

    for (j = 0; j < bufLen; j++) {
        i = ((quint16)(crc_accum >> 8) ^ buff[j]) & 0xFF;
        crc_accum = (crc_accum << 8) ^ crc_table[i];
    }

    //    return crc_accum;
    buffOut[1] = (crc_accum & 0xff00) >> 8;
    buffOut[0] = crc_accum & 0x00ff;
}

// CommanderOld *CommanderOld::instance(QObject *parent)
//{
//     if (nullptr == s_instance)
//     {
//         s_instance = new CommanderOld(parent);
//     }

//    return s_instance;
//}

void CommanderOld::setSerialPort(QSerialPort* com)
{
    m_serialPort = com;
    connect(m_serialPort, SIGNAL(readyRead()), this, SLOT(onReadReady()));
    qDebug() << "open serialport: " << m_serialPort->isOpen();
}

CommanderOld::CommanderOld(QObject* parent)
    : QObject(parent)
{
    m_nDataIndex = 0;
    //    m_bBusy = false;
    m_nQieGeCiShu = 5;
    m_serialPort = nullptr;

    initCmdNameMap();

    m_timerStartCmd = new QTimer;
    m_timerStartCmd->setSingleShot(true);
    m_timerStartCmd->setInterval(300);
    connect(m_timerStartCmd, SIGNAL(timeout()), this, SLOT(onStartCmd()));
}

void CommanderOld::stop()
{
    qDebug() << "stop cmd";
    foreach (CmdEle* cmd, m_listCmds) {
        cmd->deleteLater();
    }
    m_listCmds.clear();

    //    CmdEle* stop1 = new CmdStop1;
    CmdEle* stop2 = new CmdStop2;
    //    m_listCmds << stop1 << stop2;
    m_listCmds << stop2;

    startCmdTimer();
}

void CommanderOld::stop1()
{
    qDebug() << "stop1 cmd";
    foreach (CmdEle* cmd, m_listCmds) {
        cmd->deleteLater();
    }
    m_listCmds.clear();

    CmdEle* stop1 = new CmdStop1;
    m_listCmds << stop1;

    //    m_bBusy = false;
    startCmdTimer();
    //    m_bBusy = true;
}

void CommanderOld::stop2()
{
    qDebug() << "stop2 cmd";
    foreach (CmdEle* cmd, m_listCmds) {
        cmd->deleteLater();
    }
    m_listCmds.clear();

    CmdEle* stop2 = new CmdStop2;
    m_listCmds << stop2;

    //    m_bBusy = false;
    startCmdTimer();
    //    m_bBusy = true;
}

void CommanderOld::init()
{
    CmdEle* cmdCurrent1 = new CmdWriteCurrent1;
    cmdCurrent1->setCurrent1(1000);  // mA
    CmdEle* cmdVelocity1 = new CmdWriteVelocity1;
    cmdVelocity1->setVelocity1(15000);

    CmdEle* cmdCurrent2 = new CmdWriteCurrent2;
    cmdCurrent2->setCurrent2(1000);  // mA
    CmdEle* cmdVelocity2 = new CmdWriteVelocity2;
    cmdVelocity2->setVelocity2(2000);

    m_listCmds << cmdCurrent1 << cmdVelocity1 << cmdCurrent2 << cmdVelocity2;
    startCmdTimer();
}

void CommanderOld::repos()
{
    CmdEle* cmdRepos1 = new Repos1;
    CmdEle* cmdRepos2 = new Repos2;

    m_listCmds << cmdRepos1 << cmdRepos2;
    startCmdTimer();
}

void CommanderOld::toPos1(int n)
{
    qDebug() << "toPos1: " << n;
    CmdEle* cmdPos1 = new CmdWritePos1;
    cmdPos1->setPos1(n);

    m_listCmds << cmdPos1;
    startCmdTimer();
}

void CommanderOld::toPos2(int n)
{
    qDebug() << "toPos2: " << n;
    CmdEle* cmdPos2 = new CmdWritePos2;
    cmdPos2->setPos2(n);

    m_listCmds << cmdPos2;
    startCmdTimer();
}

void CommanderOld::jinxian()
{
    CmdEle* cmdRepos1 = new Repos1;
    CmdEle* cmdToPos2 = new CmdWritePos2;
    cmdToPos2->setPos2(122);
    //    m_listCmds << cmdRepos1 << cmdToPos2;
    m_listCmds.insert(0, cmdRepos1);
    m_listCmds.insert(1, cmdToPos2);
    startCmdTimer();
}

void CommanderOld::jiajin()
{
    CmdEle* cmdWriteCurrent2 = new CmdWriteCurrent2;
    cmdWriteCurrent2->setCurrent2(1000);
    CmdEle* cmdToPos2 = new CmdWritePos2;
    cmdToPos2->setPos2(122 + 5 * 360 + 110);
    //    m_listCmds << cmdWriteCurrent2 << cmdToPos2;
    m_listCmds.insert(0, cmdWriteCurrent2);
    m_listCmds.insert(1, cmdToPos2);
    startCmdTimer();
}

void CommanderOld::qiege()
{
    CmdEle* cmdToPos1 = new CmdWritePos1;
    cmdToPos1->setPos1(m_nAbsPos1 + 360 * m_nQieGeCiShu);
    //    m_listCmds << cmdToPos1;
    m_listCmds.insert(0, cmdToPos1);
    startCmdTimer();
}

void CommanderOld::tuihui()
{
    CmdEle* cmdWriteCurrent2 = new CmdWriteCurrent2;
    cmdWriteCurrent2->setCurrent2(1500);
    CmdEle* cmdToPos2 = new CmdWritePos2;
    cmdToPos2->setPos2(m_nAbsPos2 - 360);
    CmdEle* cmdRepos2 = new Repos2;

    CmdEle* cmdRepos1 = new Repos1;
    //    m_listCmds << cmdWriteCurrent2 << cmdToPos2 << cmdRepos1;
    m_listCmds.insert(0, cmdWriteCurrent2);
    m_listCmds.insert(1, cmdToPos2);
    m_listCmds.insert(2, cmdRepos2);
    m_listCmds.insert(3, cmdRepos1);
    startCmdTimer();
}

void CommanderOld::readI2()
{
    CmdEle* rI2 = new CmdReadCurrent2;
    m_listCmds << rI2;
    startCmdTimer();
}

void CommanderOld::readPos2()
{
    CmdEle* pos2 = new CmdReadPos2;
    m_listCmds << pos2;
    startCmdTimer();
}

void CommanderOld::readU1()
{
    CmdEle* u1 = new CmdReadVoltage1;
    m_listCmds << u1;
    startCmdTimer();
}

void CommanderOld::readI1()
{
    CmdEle* rI1 = new CmdReadCurrent1;
    m_listCmds << rI1;
    startCmdTimer();
}

void CommanderOld::readPos1()
{
    CmdEle* pos1 = new CmdReadPos1;
    m_listCmds << pos1;
    startCmdTimer();
}

void CommanderOld::readU2()
{
    CmdEle* u2 = new CmdReadVoltage2;
    m_listCmds << u2;
    startCmdTimer();
}

void CommanderOld::relativePos1(int pos)
{
    int posX = m_nAbsPos1 + pos;
    CmdEle* toPos1 = new CmdWritePos1;
    toPos1->setPos1(posX);

    //    m_listCmds.insert(0, toPos1);
    m_listCmds << toPos1;
    startCmdTimer();
}

void CommanderOld::relativePos2(int pos)
{
    qDebug() << "relativePos2: " << pos;
    int posX = m_nAbsPos2 + pos;
    qDebug() << "relativePos2: m_nAbsPos2: " << m_nAbsPos2;

    CmdEle* toPos2 = new CmdWritePos2;
    toPos2->setPos2(posX);
    //    m_listCmds.insert(0, toPos2);
    m_listCmds << toPos2;
    startCmdTimer();
}

void CommanderOld::setCurrent1(int c)
{
    CmdEle* setC1 = new CmdWriteCurrent1;
    setC1->setCurrent1(c);
    m_listCmds << setC1;
    startCmdTimer();
}

void CommanderOld::setCurrent2(int c)
{
    CmdEle* setC2 = new CmdWriteCurrent2;
    setC2->setCurrent2(c);
    m_listCmds << setC2;
    startCmdTimer();
}

void CommanderOld::setVelocity1(int v)
{
    CmdEle* setV1 = new CmdWriteVelocity1;
    setV1->setVelocity1(v);
    m_listCmds << setV1;
    startCmdTimer();
}

void CommanderOld::setVelocity2(int v)
{
    CmdEle* setV2 = new CmdWriteVelocity2;
    setV2->setVelocity2(v);
    m_listCmds << setV2;
    startCmdTimer();
}

void CommanderOld::setQieGeCiShu(int c)
{
    m_nQieGeCiShu = c;
}

quint16 CommanderOld::getCrc16(quint8* buff, quint8 bufLen)
{
    quint8 i, j;
    quint16 crc_accum = 0;
    quint16 crc_table[256] = {
        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};

    for (j = 0; j < bufLen; j++) {
        i = ((quint16)(crc_accum >> 8) ^ buff[j]) & 0xFF;
        crc_accum = (crc_accum << 8) ^ crc_table[i];
    }

    quint8 nH = (crc_accum & 0xff00) >> 8;
    quint8 nL = (crc_accum & 0x00ff);
    crc_accum = nL;
    crc_accum <<= 8;
    crc_accum |= nH;
    return crc_accum;
}

quint8 CommanderOld::parseCmd(quint8* buff, quint8 len)
{
    if (len < 8) {
        /// buff is not enough
        return 0;
    }

    int nStartIndex = 0;

    while (nStartIndex + 8 <= len) {
        if (buff[nStartIndex] != 1 && buff[nStartIndex] != 2) {
            nStartIndex += 1;
            continue;
        }

        quint8 nCrcH = buff[nStartIndex + 6];
        quint8 nCrcL = buff[nStartIndex + 7];
        quint16 nCrcGet = nCrcH;
        nCrcGet <<= 8;
        nCrcGet |= nCrcL;

        quint16 nCrc16Cal = getCrc16(buff + nStartIndex, 6);

        if (nCrcGet != nCrc16Cal) {
            qDebug() << "\n\n\n crc not right \n\n\n";
            nStartIndex += 1;
            continue;
        } else {
            QString strCmd = "";
            for (int i = 0; i < len; ++i) {
                strCmd += QString::number(buff[i], 16).rightJustified(2, '0') + " ";
            }
            qDebug() << "recv buff: " << strCmd;

            quint8 nId = buff[nStartIndex];
            quint8 nFC = buff[nStartIndex + 1];
            quint8* pData = buff + nStartIndex + 2;

            QString strCmdName = m_mapCmdName.value(nFC, "failed");
            QString strCmdNameId = QString("%1_%2").arg(strCmdName).arg(nId);

            int n32 = 0;

            if (strCmdName == "readpos" || strCmdName == "readvelocity") {
                for (int i = 3; i > -1; --i) {
                    n32 <<= 8;
                    n32 += pData[i];
                }

                int i32 = 0;
                memcpy(&i32, &n32, 4);
                if ("readpos" == strCmdName) {
                    qDebug() << "parseCmd readpos, nId: " << nId;

                    if (nId == 1) {
                        onRecvCmdEleRes(strCmdNameId, i32 / 501923.0 * 180.0);
                        m_nAbsPos1 = i32 / 501923.0 * 180.0;
                    } else if (nId == 2) {
                        onRecvCmdEleRes(strCmdNameId, i32 / 303750.0 * 180.0);
                        m_nAbsPos2 = i32 / 303750.0 * 180.0;
                        qDebug() << "parse m_nAbsPos2: " << m_nAbsPos2;
                    }
                } else if (strCmdName == "readvelocity") {
                    onRecvCmdEleRes(strCmdNameId, n32);
                }
            }

            else if (strCmdName == "readcurrent") {
                quint16 u16 = 0;
                for (int i = 1; i > -1; --i) {
                    u16 <<= 8;
                    u16 += pData[i];
                }
                qint16 n16 = 0;
                memcpy(&n16, &u16, 2);

                if (nId == 1) {
                    onRecvCmdEleRes(strCmdNameId, n16 / 310.0 * 4955.12);
                } else if (nId == 2) {
                    onRecvCmdEleRes(strCmdNameId, n16);
                }
            }

            else if (strCmdName == "readvoltage") {
                float f = pData[0] * 0.1;
                onRecvCmdEleRes(strCmdNameId, f);
            } else {
                QString strResult;
                if (pData[0] == 0) {
                    strResult = "success:";
                } else {
                    strResult = "failed:";
                }
                onRecvCmdEleRes(strResult + strCmdNameId, 0);
            }

            return nStartIndex + 8;
        }
    }

    return len;
}

void CommanderOld::initCmdNameMap()
{
    m_mapCmdName.insert(1, "writepos");
    m_mapCmdName.insert(2, "writecurrent");
    m_mapCmdName.insert(3, "writevelocity");
    m_mapCmdName.insert(4, "repos");
    m_mapCmdName.insert(6, "stop");

    m_mapCmdName.insert(0x23, "readpos");
    m_mapCmdName.insert(0x24, "readcurrent");
    m_mapCmdName.insert(0x25, "readvelocity");
    m_mapCmdName.insert(0x26, "readvoltage");

    /// repos1 current 1500mA
}

void CommanderOld::onReadReady()
{
    int lenRecv = m_serialPort->read((char*)s_buff + m_nDataIndex, BUFF_MAX_LEN);
    m_nDataIndex += lenRecv;

    int nParsed = parseCmd(s_buff, m_nDataIndex);

    qDebug() << __FUNCTION__ << "nParsed: " << nParsed;

    if (nParsed > 0) {
        qDebug() << "ok parse, nParsed: " << nParsed;
        memmove(s_buff, s_buff + nParsed, BUFF_MAX_LEN - nParsed);
        m_nDataIndex -= nParsed;

        //        m_listCmds.takeFirst()->deleteLater();
    }

    //    if (0 == nParsed)
    //    {
    //        m_listCmds.first()->incCount();
    //        if (m_listCmds.first()->count() < 2 )
    //        {
    ////            m_timerCmdWait->start(); /// restart the timer, if the data is not enough
    ////            m_bBusy = true;
    //            return;
    //        }
    //        else
    //        {
    //            m_listCmds.takeFirst()->deleteLater();
    ////            m_bBusy = false;
    //            execCmd();
    //        }
    //    }
    //    else
    //    {
    //        qDebug() << "ok parse, nParsed: " << nParsed;
    //        memmove(s_buff, s_buff+nParsed, BUFF_MAX_LEN-nParsed);
    //        m_nDataIndex -= nParsed;

    //        m_listCmds.takeFirst()->deleteLater();
    ////        m_bBusy = false;
    //        execCmd();
    //    }
}

void CommanderOld::onStartCmd()
{
    execCmd();
}

void CommanderOld::onGetParam()
{
    if (m_listCmds.size() > 20) {
        foreach (CmdEle* cmd, m_listCmds) {
            cmd->deleteLater();
        }
        m_listCmds.clear();
    }

    //    CmdEle* i1 = new CmdReadCurrent1;
    //    CmdEle* u1 = new CmdReadVoltage1;
    //    CmdEle* p1 = new CmdReadPos1;
    //    CmdEle* i2 = new CmdReadCurrent2;
    //    CmdEle* u2 = new CmdReadVoltage2;
    CmdEle* p2 = new CmdReadPos2;
    //    m_listCmds << i1 << u1 << p1 << i2 << u2 << p2;

    m_listCmds << p2;

    qDebug() << "onGetParam: cmdList size: " << m_listCmds.size();

    startCmdTimer();
}

void CommanderOld::onRecvCmdEleRes(QString cmdName, float f)
{
    qDebug() << "onRecvCmdEleRes: cmdName: " << cmdName;
    if (!cmdName.startsWith("read")) {
        qDebug() << "cmdName: " << cmdName << "   fValue: " << f;
    }

    if (cmdName.contains(":")) {
        //        QString strResult = cmdName.split(":").first();
        //        QString strTheCmd = cmdName.split(":").last();

        emit sigMsg(QVariant(cmdName));
        cmdName = cmdName.split(":").last();
    } else {
        QVariantMap mapVar;
        mapVar.insert("cmdName", cmdName);
        mapVar.insert("value", f);
        emit sigVarValue(mapVar);
    }

    for (int i = 0; i < m_listCmds.size(); ++i) {
        if (cmdName == m_listCmds.at(i)->getName()) {
            qDebug() << "delete cmd:::::::: " << cmdName;
            m_listCmds.at(i)->deleteLater();
            m_listCmds.removeAt(i);
            break;
        }
    }

    startCmdTimer();
}

void CommanderOld::startCmdTimer()
{
    m_timerStartCmd->start();
}

void CommanderOld::execCmd()
{
    if (m_listCmds.isEmpty()) {
        qDebug() << "emptyu retutrn";
        return;
    }

    CmdEle* theCmd = m_listCmds.first();
    quint8 arrCmd[10] = {0};
    memcpy(arrCmd, theCmd->getCmd(), 8);
    arrCmd[8] = '\r';
    arrCmd[9] = '\n';

    qDebug() << "execCmd e " << theCmd->getName();

    if (nullptr == m_serialPort) {
        qDebug() << "serialport is not open";
        return;
    }

    QString strCmd = "";
    for (int i = 0; i < 10; ++i) {
        strCmd += QString::number(arrCmd[i], 16).rightJustified(2, '0') + " ";
    }
    qDebug() << "execCmd: " << strCmd;

    m_serialPort->write((const char*)arrCmd, 10);
}

static const quint8 crc_hi[] = {
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81,
    0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
    0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
    0x81, 0x40, 0x01,
    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
    0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81,
    0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80,
    0x41, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01,
    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00,
    0xC1, 0x81, 0x40,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81,
    0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
    0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
    0x81, 0x40, 0x01,
    0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01,
    0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81,
    0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
    0x40, 0x01, 0xC0,
    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
    0x80, 0x41, 0x01,
    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
    0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
    0x00, 0xC1, 0x81,
    0x40};

static const quint8 crc_low[] = {
    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
    0x05, 0xC5, 0xC4,
    0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB,
    0x0B, 0xC9, 0x09,
    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE,
    0xDF, 0x1F, 0xDD,
    0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2,
    0x12, 0x13, 0xD3,
    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
    0x36, 0xF6, 0xF7,
    0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E,
    0xFE, 0xFA, 0x3A,
    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B,
    0x2A, 0xEA, 0xEE,
    0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27,
    0xE7, 0xE6, 0x26,
    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
    0x63, 0xA3, 0xA2,
    0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD,
    0x6D, 0xAF, 0x6F,
    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8,
    0xB9, 0x79, 0xBB,
    0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4,
    0x74, 0x75, 0xB5,
    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
    0x50, 0x90, 0x91,
    0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94,
    0x54, 0x9C, 0x5C,
    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59,
    0x58, 0x98, 0x88,
    0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D,
    0x4D, 0x4C, 0x8C,
    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
    0x41, 0x81, 0x80,
    0x40};

quint8* CmdEle::getCmd()
{
    return m_buff;
}

void CmdEle::setCurrent1(float f)
{
    float f1 = f * 310.0 / 4955.12;
    m_nParam = f1;

    buildCmd();
}

void CmdEle::setPos1(float f)
{
    float f1 = 501923.0 / 180.0 * f;
    //    m_nParam = f1;

    int n1 = f1;
    //    m_nParam = f1;

    memcpy(&m_nParam, &n1, 4);

    qDebug() << "setPos1 m_nParam: " << m_nParam;
    qDebug() << (m_nParam & 0xff000000);
    qDebug() << (m_nParam & 0xff0000);
    qDebug() << (m_nParam & 0xff00);
    qDebug() << (m_nParam & 0xff);

    buildCmd();
}

void CmdEle::setVelocity1(float f)
{
    m_nParam = f;
    buildCmd();
}

void CmdEle::setCurrent2(float f)
{
    m_nParam = f;
    buildCmd();
}

void CmdEle::setPos2(float f)
{
    float f1 = 303750.0 / 180.0 * f * 2;
    int n1 = f1;
    //    m_nParam = f1;

    qDebug() << "setPos2: f1" << f1 << "   n1:" << n1;

    memcpy(&m_nParam, &n1, 4);

    qDebug() << "setPos2: m_nParam: " << m_nParam;

    buildCmd();
}

void CmdEle::setVelocity2(float f)
{
    m_nParam = f;

    buildCmd();
}

void CmdEle::incCount()
{
    m_nVisitCnt += 1;
}

quint8 CmdEle::count()
{
    return m_nVisitCnt;
}

// void CmdEle::getCrc16( quint8* buff, quint8 bufLen, quint8* buffOut )
//{

//    quint8 crc_hi_byte = 0xFF;
//    quint8 crc_low_byte = 0xFF;
//    int nIndex;

//    while ( bufLen-- )
//    {
//        nIndex = crc_low_byte ^ *(buff++);
//        crc_low_byte = crc_hi_byte ^ crc_hi[nIndex];
//        crc_hi_byte = crc_low[nIndex];
//    }

//    buffOut[0] = crc_hi_byte;
//    buffOut[1] = crc_low_byte;
//}

void CmdEle::getCrc16(quint8* buff, quint8 bufLen, quint8* buffOut)
{
    quint8 i, j;
    quint16 crc_accum = 0;
    quint16 crc_table[256] = {
        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};

    for (j = 0; j < bufLen; j++) {
        i = ((quint16)(crc_accum >> 8) ^ buff[j]) & 0xFF;
        crc_accum = (crc_accum << 8) ^ crc_table[i];
    }

    //    return crc_accum;
    buffOut[1] = (crc_accum & 0xff00) >> 8;
    buffOut[0] = crc_accum & 0x00ff;
}

// quint16 CmdEle::getCrc16(quint8 *buff, quint8 bufLen)
//{

//    quint8 crc_hi_byte = 0xFF;
//    quint8 crc_low_byte = 0xFF;
//    int nIndex;

//    while ( bufLen-- )
//    {
//        nIndex = crc_low_byte ^ *(buff++);
//        crc_low_byte = crc_hi_byte ^ crc_hi[nIndex];
//        crc_hi_byte = crc_low[nIndex];
//    }

//    quint16 n16 = crc_hi_byte;
//    n16 <<= 8;
//    n16 |= crc_low_byte;
//    return n16;
//}

quint16 CmdEle::getCrc16(quint8* buff, quint8 bufLen)
{
    quint8 i, j;
    quint16 crc_accum = 0;
    quint16 crc_table[256] = {
        0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
        0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
        0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
        0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
        0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
        0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
        0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
        0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
        0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
        0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
        0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
        0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
        0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
        0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
        0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
        0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
        0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
        0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
        0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
        0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
        0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
        0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
        0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
        0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
        0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
        0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
        0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
        0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
        0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
        0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
        0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
        0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202};

    for (j = 0; j < bufLen; j++) {
        i = ((quint16)(crc_accum >> 8) ^ buff[j]) & 0xFF;
        crc_accum = (crc_accum << 8) ^ crc_table[i];
    }

    quint8 nH = (crc_accum & 0xff00) >> 8;
    quint8 nL = (crc_accum & 0x00ff);
    crc_accum = nL;
    crc_accum <<= 8;
    crc_accum |= nH;
    return crc_accum;
}

void CmdEle::getCmd(quint8* buffIn, quint8 bufLen)
{
    getCrc16(buffIn, bufLen - 2, buffIn + bufLen - 2);
}

QString CmdEle::getName()
{
    return m_strCmdName;
}

void CmdStop1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdStop2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x6, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadPos1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x11, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadPos2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x11, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadCurrent1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x12, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadCurrent2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x12, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadVelocity1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x13, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadVelocity2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x13, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadVoltage1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x14, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdReadVoltage2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x14, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void CmdWritePos1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    memcpy(cmd + 2, &m_nParam, 4);

    getCmd(cmd, 8);

    memcpy(&m_buff, cmd, 8);
}

void CmdWritePos2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x1, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    memcpy(cmd + 2, &m_nParam, 4);

    getCmd(cmd, 8);

    qDebug() << "CmdWritePos2::buildCmd";
    QString strCmd = "";
    for (int i = 0; i < 8; ++i) {
        strCmd += QString::number(cmd[i], 16).rightJustified(2, '0') + " ";
    }
    qDebug() << strCmd;

    memcpy(&m_buff, cmd, 8);
}

void CmdWriteCurrent1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    memcpy(cmd + 2, &m_nParam, 4);

    qDebug() << "m_nParam: " << m_nParam;

    getCmd(cmd, 8);

    qDebug() << "CmdWriteCurrent1::buildCmd";
    QString strCmd = "";
    for (int i = 0; i < 8; ++i) {
        strCmd += QString::number(cmd[i], 16).rightJustified(2, '0') + " ";
    }
    qDebug() << strCmd;

    memcpy(&m_buff, cmd, 8);
}

void CmdWriteCurrent2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x2, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    memcpy(cmd + 2, &m_nParam, 4);

    getCmd(cmd, 8);

    qDebug() << "CmdWriteCurrent2::buildCmd";
    QString strCmd = "";
    for (int i = 0; i < 8; ++i) {
        strCmd += QString::number(cmd[i], 16).rightJustified(2, '0') + " ";
    }
    qDebug() << strCmd;

    memcpy(&m_buff, cmd, 8);
}

void CmdWriteVelocity1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    memcpy(cmd + 2, &m_nParam, 4);

    getCmd(cmd, 8);

    qDebug() << "CmdWriteVelocity1::buildCmd";
    QString strCmd = "";
    for (int i = 0; i < 8; ++i) {
        strCmd += QString::number(cmd[i], 16).rightJustified(2, '0') + " ";
    }
    qDebug() << strCmd;

    memcpy(&m_buff, cmd, 8);
}

void CmdWriteVelocity2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    memcpy(cmd + 2, &m_nParam, 4);

    getCmd(cmd, 8);

    qDebug() << "CmdWriteVelocity2::buildCmd";
    QString strCmd = "";
    for (int i = 0; i < 8; ++i) {
        strCmd += QString::number(cmd[i], 16).rightJustified(2, '0') + " ";
    }
    qDebug() << strCmd;

    memcpy(&m_buff, cmd, 8);
}

void Repos1::buildCmd()
{
    quint8 cmd[8] = {0x1, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}

void Repos2::buildCmd()
{
    quint8 cmd[8] = {0x2, 0x4, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
    getCmd(cmd, 8);
    memcpy(&m_buff, cmd, 8);
}
